/***************************************************************************************************************************
* height_controller_cascade_PID.h
*
* Author: SFL
*
* Update Time: 2021.6.11
*
* Introduction:  Height Controller using PID (P for pos loop, pid for vel loop)
*         1. Similiar to the position controller in PX4 (1.8.2)
*         2. Ref to : https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/PositionControl.cpp
*         3. Here we didn't consider the mass of the drone, we treat accel_sp is the thrust_sp.
*         4. 没有考虑积分器清零的情况，在降落时 或者突然换方向机动时，积分器需要清0
*         5. 推力到欧拉角基本与PX4吻合，但是在极端情况下不吻合。如：z轴期望值为-100时。
***************************************************************************************************************************/

#ifndef HEIGHT_CONTROLLER_CASCADE_PID_H
#define HEIGHT_CONTROLLER_CASCADE_PID_H

#include <Eigen/Eigen>
#include <math.h>
#include <math_utils.h>
#include <px4_command_utils.h>
#include <wrzf_pkg/DroneState.h>
#include <wrzf_pkg/TrajectoryPoint.h>
// #include <wrzf_pkg/AttitudeReference.h>
// #include <wrzf_pkg/ControlOutput.h>

using namespace std;

class height_controller_cascade_PID
{
public:
    height_controller_cascade_PID(void):
        height_cascade_pid_nh("~")
    {
        height_cascade_pid_nh.param<float>("Height_cascade_pid/Kp_z", Kp_z, 1.0);
        height_cascade_pid_nh.param<float>("Height_cascade_pid/Kp_vz", Kp_vz, 0.4);
        height_cascade_pid_nh.param<float>("Height_cascade_pid/Ki_vz", Ki_vz, 0.02);
        height_cascade_pid_nh.param<float>("Height_cascade_pid/Kd_vz", Kd_vz, 0.15);
        height_cascade_pid_nh.param<float>("Height_cascade_pid/Hover_throttle", Hover_throttle, 0.5);
        height_cascade_pid_nh.param<float>("Height_cascade_pid/MPC_VELD_LP", MPC_VELD_LP, 5.0);

        height_cascade_pid_nh.param<float>("Height_cascade_pid/Z_VEL_MAX", MPC_Z_VEL_MAX, 0.5);
        height_cascade_pid_nh.param<float>("Height_cascade_pid/THR_MIN", MPC_THR_MIN, 0.1);
        height_cascade_pid_nh.param<float>("Height_cascade_pid/THR_MAX", MPC_THR_MAX, 0.9);
        // height_cascade_pid_nh.param<float>("Limit/tilt_max", tilt_max, 5.0);

        vel_setpoint    = 0.0;
        thrust_sp       = 0.0;
        vel_P_output    = 0.0;
        thurst_int      = 0.0;
        vel_D_output    = 0.0;
        error_vel_dot_last  = 0.0;
        error_vel_last      = 0.0;
        error_vel       = 0.0;
        vel_error_deriv = 0.0;
        delta_time      = 0.02;
    }
    // PID parameter for the control law
    
    float Kp_z;
    float Kp_vz;
    float Ki_vz;
    float Kd_vz;

    //Limitation of the velocity
    // float MPC_XY_VEL_MAX;
    float MPC_Z_VEL_MAX;

    //Hover thrust of drone (decided by the mass of the drone)
    float Hover_throttle;

    //Limitation of the thrust
    float MPC_THR_MIN;
    float MPC_THR_MAX;

    //Limitation of the tilt angle (roll and pitch)  [degree]
    // float tilt_max;

    //输出

    //Desired position and velocity of the drone
    float vel_setpoint;

    //Desired thurst of the drone[the output of this class]
    float thrust_sp;

    //Output of the vel loop in PID [thurst_int is the I]
    float vel_P_output;
    float thurst_int;
    float vel_D_output;

    float MPC_VELD_LP;

    //The delta time between now and the last step
    float delta_time;

    //Derriv of the velocity error in last step [used for the D-output in vel loop]
    float error_vel_dot_last;
    float error_vel_last;

    float error_vel;
    float vel_error_deriv;

    //Current state of the drone
    // mavros_msgs::State current_state;

    //Printf the PID parameter
    // void printf_param();

    //Printf the control result
    // void printf_result();

    // Position control main function 
    // [Input: Current state, Reference state, _Reference_State.Sub_mode, dt; Output: thrust_sp;]
    float pos_controller(const wrzf_pkg::DroneState& _DroneState, const wrzf_pkg::TrajectoryPoint& _Reference_State, float dt);

    //Position control loop [Input: current pos, desired pos; Output: desired vel]
    void _positionController(const wrzf_pkg::DroneState& _DroneState, const wrzf_pkg::TrajectoryPoint& _Reference_State, float& vel_setpoint);

    //Velocity control loop [Input: current vel, desired vel; Output: desired thrust]
    void _velocityController(const wrzf_pkg::DroneState& _DroneState, const wrzf_pkg::TrajectoryPoint& _Reference_State, float& thrust_sp);

    void cal_vel_error_deriv(const float& error_now, float& vel_error_deriv);


    // virtual ~height_controller_cascade_PID();

private:
    ros::NodeHandle height_cascade_pid_nh;
};

float height_controller_cascade_PID::pos_controller(const wrzf_pkg::DroneState& _DroneState, 
                    const wrzf_pkg::TrajectoryPoint& _Reference_State, 
                    float dt)
{
    delta_time = dt;

    _positionController(_DroneState, _Reference_State, vel_setpoint);

    float thrust_sp;

    _velocityController(_DroneState, _Reference_State, thrust_sp);

    return thrust_sp;
}

void height_controller_cascade_PID::_positionController(const wrzf_pkg::DroneState& _DroneState, 
                        const wrzf_pkg::TrajectoryPoint& _Reference_State, 
                        float& vel_setpoint)
{
    vel_setpoint = Kp_z  * (_Reference_State.position_ref[2] - _DroneState.position[2]);
    vel_setpoint = constrain_function(vel_setpoint, MPC_Z_VEL_MAX);
}

void height_controller_cascade_PID::_velocityController(const wrzf_pkg::DroneState& _DroneState, 
                        const wrzf_pkg::TrajectoryPoint& _Reference_State, float& thrust_sp)
{
    error_vel = vel_setpoint - _DroneState.velocity[2];
    vel_P_output = Kp_vz  * error_vel;
    cal_vel_error_deriv(error_vel, vel_error_deriv);
    vel_D_output = Kd_vz  * vel_error_deriv;
    float thrust_desired_Z  = vel_P_output + thurst_int + vel_D_output + Hover_throttle;
    bool stop_integral_Z = ( thrust_desired_Z  >= MPC_THR_MAX && error_vel >= 0.0f) ||
                           ( thrust_desired_Z  <= MPC_THR_MIN && error_vel <= 0.0f);
    if (!stop_integral_Z) 
    {
        thurst_int += Ki_vz  * error_vel * delta_time;

        // limit thrust integral
        thurst_int = min(fabs(thurst_int), MPC_THR_MAX ) * sign_function(thurst_int);
    }

    // Saturate thrust setpoint in Z-direction.
    thrust_sp = constrain_function2(thrust_desired_Z , MPC_THR_MIN, MPC_THR_MAX);

}


void height_controller_cascade_PID::cal_vel_error_deriv(const float& error_now, float& vel_error_deriv)
{
    float error_vel_dot_now;
    error_vel_dot_now = (error_now - error_vel_last)/delta_time;

    error_vel_last = error_now;
    float a,b;
    b = 2 * M_PI * MPC_VELD_LP * delta_time;
    a = b / (1 + b);

    vel_error_deriv = a * error_vel_dot_now + (1 - a) * error_vel_dot_last ;

    error_vel_dot_last = vel_error_deriv;
}






#endif //HEIGHT_CONTROLLER_CASCADE_PID_H
